#include "dm_imu.h"
#include "can_common.h"
#include "drivers/dev_can.h"
#include "rtthread.h"
#include "rttypes.h"
#include "robotdef.h"

dm_imu_t dm_imu;

#define LOG_TAG              "dmimu"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


/**
************************************************************************
* @brief:      	float_to_uint: 浮点数转换为无符号整数函数
* @param[in]:   x_float:	待转换的浮点数
* @param[in]:   x_min:		范围最小值
* @param[in]:   x_max:		范围最大值
* @param[in]:   bits: 		目标无符号整数的位数
* @retval:     	无符号整数结果
* @details:    	将给定的浮点数 x 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个指定位数的无符号整数
************************************************************************
**/
int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
	/* Converts a float to an unsigned int, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
/**
************************************************************************
* @brief:      	uint_to_float: 无符号整数转换为浮点数函数
* @param[in]:   x_int: 待转换的无符号整数
* @param[in]:   x_min: 范围最小值
* @param[in]:   x_max: 范围最大值
* @param[in]:   bits:  无符号整数的位数
* @retval:     	浮点数结果
* @details:    	将给定的无符号整数 x_int 在指定范围 [x_min, x_max] 内进行线性映射，映射结果为一个浮点数
************************************************************************
**/
float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
	/* converts unsigned int to float, given range and number of bits */
	float span = x_max - x_min;
	float offset = x_min;
	return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}



void IMU_RequestData(uint16_t can_id,uint8_t reg)
{
	dm_imu.CanMessage.txmsg.len=4;
	dm_imu.CanMessage.txmsg.ide=RT_CAN_STDID;
	dm_imu.CanMessage.txmsg.rtr=RT_CAN_DTR;
	dm_imu.CanMessage.txmsg.id=0x6FF;
    dm_imu.CanMessage.txmsg.data[0] = (rt_uint8_t)can_id;
    dm_imu.CanMessage.txmsg.data[1] = (rt_uint8_t)(can_id>>8);
    dm_imu.CanMessage.txmsg.data[2] = reg;
    dm_imu.CanMessage.txmsg.data[3] = 0XCC;

    if (dm_imu.CanMessage.can_handle != RT_NULL)
    {
        can_send(*dm_imu.CanMessage.can_handle,&dm_imu.CanMessage.txmsg);
        //rt_device_write(*dm_imu.CanMessage.can_handle,0,&dm_imu.CanMessage.txmsg,sizeof(dm_imu.CanMessage.txmsg));
    }
}


void IMU_UpdateAccel(struct rt_can_msg *canmsg)
{
	uint16_t accel[3];
	
	accel[0]=canmsg->data[3]<<8|canmsg->data[2];
	accel[1]=canmsg->data[5]<<8|canmsg->data[4];
	accel[2]=canmsg->data[7]<<8|canmsg->data[6];
	
	dm_imu.accel[0]=uint_to_float(accel[0],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
	dm_imu.accel[1]=uint_to_float(accel[1],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
	dm_imu.accel[2]=uint_to_float(accel[2],ACCEL_CAN_MIN,ACCEL_CAN_MAX,16);
	
}

void IMU_UpdateGyro(struct rt_can_msg *canmsg)
{
	uint16_t gyro[3];
	
	gyro[0]=canmsg->data[3]<<8|canmsg->data[2];
	gyro[1]=canmsg->data[5]<<8|canmsg->data[4];
	gyro[2]=canmsg->data[7]<<8|canmsg->data[6];
	
	dm_imu.gyro[0]=uint_to_float(gyro[0],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
	dm_imu.gyro[1]=uint_to_float(gyro[1],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
	dm_imu.gyro[2]=uint_to_float(gyro[2],GYRO_CAN_MIN,GYRO_CAN_MAX,16);
}


void IMU_UpdateEuler(struct rt_can_msg *canmsg)
{
	int euler[3];
	
	euler[0]=canmsg->data[3]<<8|canmsg->data[2];
	euler[1]=canmsg->data[5]<<8|canmsg->data[4];
	euler[2]=canmsg->data[7]<<8|canmsg->data[6];
	
	dm_imu.pitch=uint_to_float(euler[0],PITCH_CAN_MIN,PITCH_CAN_MAX,16);
	dm_imu.yaw=uint_to_float(euler[1],YAW_CAN_MIN,YAW_CAN_MAX,16);
	dm_imu.roll=uint_to_float(euler[2],ROLL_CAN_MIN,ROLL_CAN_MAX,16);

    // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
    if (dm_imu.yaw - dm_imu.yawlast > 180.0f){dm_imu.yaw_round--;}
    else if (dm_imu.yaw - dm_imu.yawlast < -180.0f){dm_imu.yaw_round++;}
    dm_imu.YawTotalAngle = 360.0f * dm_imu.yaw_round + dm_imu.yaw;
    dm_imu.yawlast = dm_imu.yaw;
}


void IMU_UpdateQuaternion(struct rt_can_msg *canmsg)
{
	int w = canmsg->data[1]<<6| ((canmsg->data[2]&0xF8)>>2);
	int x = (canmsg->data[2]&0x03)<<12|(canmsg->data[3]<<4)|((canmsg->data[4]&0xF0)>>4);
	int y = (canmsg->data[4]&0x0F)<<10|(canmsg->data[5]<<2)|(canmsg->data[6]&0xC0)>>6;
	int z = (canmsg->data[6]&0x3F)<<8|canmsg->data[7];
	
	dm_imu.q[0] = uint_to_float(w,Quaternion_MIN,Quaternion_MAX,14);
	dm_imu.q[1] = uint_to_float(x,Quaternion_MIN,Quaternion_MAX,14);
	dm_imu.q[2] = uint_to_float(y,Quaternion_MIN,Quaternion_MAX,14);
	dm_imu.q[3] = uint_to_float(z,Quaternion_MIN,Quaternion_MAX,14);
}

void IMU_UpdateData(struct rt_can_msg *canmsg)
{
    if (canmsg->id == dm_imu.CanMessage.rx_id)
    {
        switch(canmsg->data[0])
        {
            case 1:
                IMU_UpdateAccel(canmsg);
                break;
            case 2:
                IMU_UpdateGyro(canmsg);
                break;
            case 3:
                IMU_UpdateEuler(canmsg);
                break;
            case 4:
                IMU_UpdateQuaternion(canmsg);
                break;
        }
    }
}

static void dmimutx(void *parameter)
{
    while (1) 
    {
        IMU_RequestData(dm_imu.CanMessage.tx_id,DM_RID_GYRO);
        rt_thread_mdelay(1);
        IMU_RequestData(dm_imu.CanMessage.tx_id,DM_RID_EULER);
        rt_thread_mdelay(1);
    }
}

int dm_imu_init()
{
    dm_imu.CanMessage.can_handle = &can2_bus;
    dm_imu.CanMessage.tx_id = 0x01;
    dm_imu.CanMessage.rx_id = 0x11;
    
    rt_uint8_t ret =RT_EOK;

    /* 创建 imu task 线程 */
    rt_thread_t thread = rt_thread_create("dmimu_tx", dmimutx, RT_NULL, 2048, 14, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
        LOG_D("dm_imu thread init success!\n");
    }
    else
    {
        ret = RT_ERROR;
    }

    return  ret;
}
#if defined (GIMBAL_BOARD)
INIT_APP_EXPORT(dm_imu_init);
#endif